 /****************************************************************************\
* @file     DriverBoard.h 
* @author   傅帅
* @date     2019/12/30
* @brief    舵向电机
* @note     这版设计使用单独C610/620驱动板,两板间交流方式为电机相对于逻辑0位的绝对计数
            极大简化了逻辑,同时可以更好地保证C610/620控制的安全性
******************************************************************************
* @attent   维护最多16个C610/620的驱动，编号0-7 8-15分别在CAN1 CAN2
            可接受两路串口消息,处理逻辑相同,从哪个串口初始化的返回也发回那个串口
            PID的目标值为相对于逻辑零位的总counts
            
            通讯协议如下:
            主控下发:
            1.初始化帧: 对单个电机进行使能和速度环位置环PID参数设置
                       'A'+0x01+电机编号(char 0-15)
                        +设置零点uint16_t counts+最小安全计数uint32_t counts+最大安全计数uint32_t counts
                        +p+i+d+i_lim+total_lim(vel)
                        +p+i+d+i_lim+total_lim(pos)+校验

            2.控制帧:  对单个电机进行速度/位置/电流控制
                       'A'+0x02+电机编号+'v'/'p'/'c'+int32_t 目标值+校验

            3.归零帧:  告知该电机已回到逻辑零位附近,收到该帧则将累计圈数清零
                       'A'+0x03+电机编号+校验           
                       
            驱动上发:
            1.电机状态: 电机当前的绝对计数(以逻辑0位为0顺正逆负的count)
                       'A'+0x00+电机编号+int32_t count+校验
            2.初始化帧应答: 收到初始化帧后回发消息，保证初始化成功
                       'A'+0x01+校验
            3.归零帧应答: 收到归零帧后回发消息，保证归零成功
                       'A'+0x03+校验            
****************************************************************************/
#include "BR_Usart.h"
#include "BR_Misc.h"
#include "stm32f4xx.h"
#include "DriverBoard.h"
#include "Config.h"
#include "can.h"
#include "delay.h"
#include "string.h"

Motor_t MotorsTbl[16];        // 电机结构体数组本体
static uint8_t prvTxBuffer[MAX_TX_LEN];
static Motor_t prvUsartOneMotorList, prvUsartTwoMotorList;        // 两串口使能列表,两串口请勿控制同一个电机
static uint8_t prvCurrentSendBuf[32];   // 用于最终发送的缓冲区,内存格式应整理为高位在前
static uint8_t prvC6120GroupUsed[4];    // 用于标记被管理的C610/620编号，判断是否向CAN上发消息
static void prvOnUsartOneRecv(uint8_t *pData, uint32_t Len);
static void prvOnUsartTwoRecv(uint8_t *pData, uint32_t Len);
static void prvMotorListInsert(Motor_t *pUsartXMotorList, Motor_t *pMotorToInsert);
static void prvSafeCheck(Motor_t *pMotor);  // 安全检查
static void prvOnUsartXRecv(eUsart_t UsartX, uint8_t *pData, uint32_t Len);     // 串口接收逻辑
static void prvUsartXMotorListLoopOnce(Motor_t *pUsartXMotorList);              // 主循环逻辑
static void prvCANX_DataHandle(CAN_TypeDef *CANX);                              // CAN处理逻辑
static void prvCurrentSend(void);                                               // 最终CAN发送逻辑

/* -------------------------------------------------- Public ----------------------------------------------- */
/** @brief  主循环 **/
void DriverBoardMainLoop(void)
{
    while(1)
    {
        prvUsartXMotorListLoopOnce(&prvUsartOneMotorList);
        prvUsartXMotorListLoopOnce(&prvUsartTwoMotorList);        
        prvCurrentSend();
        delay_ms(1000/configMAIN_LOOP_RATE);
    }
}

/** @brief  初始化 **/
void DriverBoardInit(void)
{
    BR_vUsartInitWithDMA(USART_ONE, 1382400, MAX_TX_LEN, 2, MAX_RX_LEN, 3, prvOnUsartOneRecv);
    BR_vUsartInitWithDMA(USART_TWO, 1382400, MAX_TX_LEN, 2, MAX_RX_LEN, 3, prvOnUsartTwoRecv);
    CAN_Config(CAN1, APB1_45MHZ_1M, GPIOA, GPIO_Pin_11, GPIO_Pin_12);    // CAN1初始化
    CAN_Config(CAN2, APB1_45MHZ_1M, GPIOB, GPIO_Pin_12, GPIO_Pin_13);    // CAN2初始化
}

/* -------------------------------------------------- Private ----------------------------------------------- */
/** @brief  串口One回调 **/
static void prvOnUsartOneRecv(uint8_t *pData, uint32_t Len)
{ 
    prvOnUsartXRecv(USART_ONE, pData, Len);
}

/** @brief  串口Two回调 **/
static void prvOnUsartTwoRecv(uint8_t *pData, uint32_t Len)
{
    prvOnUsartXRecv(USART_TWO, pData, Len);
}

/** @brief  串口空闲中断中进行消息解析 **/
static void prvOnUsartXRecv(eUsart_t UsartX, uint8_t *pData, uint32_t Len)
{
    uint8_t Sum, *pMsg;          // 局部指针用于消息处理
    pMsg = pData;         
    if(*pMsg != 'A')return;   // 第一个字节若非attention，等下一帧
    Sum = 0;
    for(;pMsg<pData+Len-1;pMsg++)Sum += *pMsg;   // 到校验位前全加起来
    if((uint8_t)Sum == *pMsg)   // 若校验通过
    {
        pMsg = pData+1;         // 重新回到A后一字节
        uint8_t Func = *pMsg++;
        uint8_t MotorIndex = *pMsg++;
        Motor_t *pMotor = &MotorsTbl[MotorIndex];
        switch(Func)
        {
            case 0x01:          // 初始化帧
            {         
                uint8_t ListInsertFlag = (MotorsTbl[MotorIndex].Usart == 0) ? 1 : 0;    // 第一次初始化时才将链表插入标志置1
                prvC6120GroupUsed[MotorIndex/4] = 1;        // 标记该组已有电机被管理，可以向CAN上发送消息
                pMotor->ZeroCounts = *(uint16_t *)pMsg;     // 记录逻辑零位处的Counts
                pMsg += 2;
                pMotor->MinSafePos = *(int32_t *)pMsg;      // 记录最小安全Counts
                pMsg += 4;                
                pMotor->MaxSafePos = *(int32_t *)pMsg;      // 记录最大安全Counts
                pMsg += 4;                
                float Pars[10];
                memcpy(Pars, pMsg, 40);
                /* 初始化对应电机 */
                pMotor->Usart = UsartX;
                pMotor->MotorIndex = MotorIndex;
                pMotor->ControlState = POS;         // 默认位置控制
                pMotor->Circles = 0;                // 圈数清零
                pMotor->PID_Pos.target_value = 0;   // 目标位置清零
                PID_StructInit(&pMotor->PID_Vel);
                PID_StructInit(&pMotor->PID_Pos);
                pMotor->PID_Vel.f_ParamInit( &pMotor->PID_Vel, Pars[0], Pars[1], Pars[2], Pars[3], Pars[4] );
                pMotor->PID_Pos.f_ParamInit( &pMotor->PID_Pos, Pars[5], Pars[6], Pars[7], Pars[8], Pars[9] );
                /* 将被初始化的电机挂到对应串口链表下 */
                if(ListInsertFlag)  // 插入链表标志为1时才插入链表
                {
                    if(UsartX == USART_ONE)prvMotorListInsert(&prvUsartOneMotorList, &MotorsTbl[MotorIndex]);
                    else prvMotorListInsert(&prvUsartTwoMotorList, &MotorsTbl[MotorIndex]);
                }
                /* 回发初始化帧应答消息 */
                uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof(prvTxBuffer), "A%c", 0x01);
                BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);               // 校验位计算并加入
                while(BR_iSend(pMotor->Usart, prvTxBuffer, TxLen+1)){delay_ms(1);}          // 发送     
                break;
            }
            case 0x02:          // 控制帧
            {
                switch(*pMsg++)
                {
                    case 'v':
                        pMotor->ControlState = VEL;
                        pMotor->PID_Vel.target_value = *(int16_t *)pMsg;
                        break;
                    case 'p':
                    {
                        pMotor->ControlState = POS;
                        pMotor->PID_Pos.target_value = *(int32_t *)pMsg;
                        break;
                    }
                    case 'c':
                        pMotor->ControlState = CUR;  
                        pMotor->CurrentSet = *(uint16_t *)pMsg;
                }
                break;
            }
            case 0x03:          // 归零帧
                pMotor->Circles = 0;
                pMotor->ControlState = POS;
                pMotor->PID_Pos.target_value = 0;
                /* 回发归零帧应答消息 */
                uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof(prvTxBuffer), "A%c", 0x03);
                BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);               // 校验位计算并加入
                while(BR_iSend(pMotor->Usart, prvTxBuffer, TxLen+1)){delay_ms(1);}          // 发送  
                break;
        }
    }
}

/** @brief  遍历UsartXMotorList的单次主循环 **/
static void prvUsartXMotorListLoopOnce(Motor_t *pUsartXMotorList)
{
    /* 遍历通过USARTX初始化的电机 */
    Motor_t *pMotor = pUsartXMotorList;
    int16_t OutPutCur = 0;  // 最终输出电流
    int16_t ListIndex = 0; // 表示当前遍历到链表下第几个 
    while(pMotor->pNext != NULL)
    {
        // 拿到一个电机
        pMotor = pMotor->pNext;
        // PID计算
        switch(pMotor->ControlState){
            case POS:
                pMotor->PID_Pos.f_Calculate(&pMotor->PID_Pos, pMotor->CurPos);  // 位置环计算
                pMotor->PID_Vel.target_value = pMotor->PID_Pos.output;          // 结果速度作为速度环目标
                pMotor->PID_Vel.f_Calculate(&pMotor->PID_Vel, pMotor->CurVel);  // 速度环计算
                OutPutCur = pMotor->PID_Vel.output;
                break;
            case VEL:
                pMotor->PID_Vel.f_Calculate(&pMotor->PID_Vel, pMotor->CurVel);  // 速度环计算
                OutPutCur = pMotor->PID_Vel.output;
                break;
            case CUR:
                OutPutCur = pMotor->CurrentSet;
                break;
        }
        // 更新要发送的电流值,发送操作在遍历两链表结束，全部更新完成后进行
        prvCurrentSendBuf[(pMotor->MotorIndex)*2] = OutPutCur>>8 & 0xFF;
        prvCurrentSendBuf[(pMotor->MotorIndex)*2 + 1] = OutPutCur & 0xFF;
        // 安全监测(回零点附近(小于MAX/2)时不进行检测)
        if(pMotor->PID_Pos.target_value > pMotor->MaxSafePos/2 || pMotor->PID_Pos.target_value < pMotor->MinSafePos/2)prvSafeCheck(pMotor);
        // 循环上发当前绝对Counts : 此处如此设计是因为DMA连续发送会导致串口传送速度跟不上,所以每个串口每次遍历都只上发一次,上发的电机循环选取
        if(pUsartXMotorList->Circles == ListIndex)  // 当前帧应上发到主控的电机在链表中的下标 == 当前链表遍历到的下标
        {
            uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof(prvTxBuffer), "A%c%c%d", 0x00, pMotor->MotorIndex, pMotor->CurPos);
            BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);               // 校验位计算并加入
            while(BR_iSend(pMotor->Usart, prvTxBuffer, TxLen+1)){delay_ms(1);}        // 发送            
        } 
        ListIndex++;
    }
    // 循环选取要上发的电机
    pUsartXMotorList->Circles += 1;
    if(pUsartXMotorList->Circles >= pUsartXMotorList->LastCounts)pUsartXMotorList->Circles = 0;
}

/** @brief  CAN1中断处理 */
void CAN1_RX0_IRQHandler(void)
{
    prvCANX_DataHandle(CAN1);  // 根据当前帧的仲裁位id处理数据        
}

/** @brief  CAN2中断处理 */
void CAN2_RX0_IRQHandler(void)
{
    prvCANX_DataHandle(CAN2);  // 根据当前帧的仲裁位id处理数据    
}

/** @brief  CAN解析 */
static void prvCANX_DataHandle(CAN_TypeDef *CANX)
{
    uint32_t StdId = 0;
    uint8_t RecvLen = 0;
    uint8_t Data8[8];
    int16_t Data16[4];
    CAN_RxMsg(CANX, &StdId, Data8, &RecvLen);
    for(int i = 0; i < 4; i++)
        Data16[i] = Data8[i * 2] << 8 | Data8[i * 2 + 1];  // 整理u8为u16
    uint8_t MotorId = (StdId&0x00F) - 1;                              // 0-7的Id
    uint8_t MotorIndex = (CANX == CAN1) ? MotorId : 8 + MotorId;      // 0-15的Index
    Motor_t *pMotor = &MotorsTbl[MotorIndex];    // 拿到这帧CAN对应的Motor
    pMotor->CurRelativePos = Data16[0];
    int16_t Vel = Data16[1];
    int16_t Torque = Data16[2];
    /* 圈数增减判断 */
    if(pMotor->CurRelativePos < 4096){
        if(pMotor->LastCounts >= 4096 && Vel > 0)pMotor->Circles += 1;
    }
    else {
        if(pMotor->LastCounts  <= 4096 && Vel < 0)pMotor->Circles -= 1;       
    }
    // 更新电机变量
    pMotor->CurPos = pMotor->Circles * 8192 + pMotor->CurRelativePos - pMotor->ZeroCounts;
    pMotor->CurVel = Vel;
    pMotor->CurTorque = Torque;
    pMotor->LastCounts = pMotor->CurRelativePos;   
}

/** @brief  安全监测,有危险情况直接将发送电流清零 */
static void prvSafeCheck(Motor_t *pMotor)
{
    if(pMotor->CurPos > pMotor->MaxSafePos || pMotor->CurPos < pMotor->MinSafePos)
    {
        prvCurrentSendBuf[(pMotor->MotorIndex)*2] = 0;
        prvCurrentSendBuf[(pMotor->MotorIndex)*2 + 1] = 0;
    }        
}

/** @brief  最终电流发送，仅当各组内有电机被管理时才向总线发送消息 */
static void prvCurrentSend(void)
{
    if(prvC6120GroupUsed[0])CAN_TxMsg(CAN1, 0x200, &prvCurrentSendBuf[0], 8);
    if(prvC6120GroupUsed[1])CAN_TxMsg(CAN1, 0x1FF, &prvCurrentSendBuf[8], 8);
    if(prvC6120GroupUsed[2])CAN_TxMsg(CAN2, 0x200, &prvCurrentSendBuf[16], 8);
    if(prvC6120GroupUsed[3])CAN_TxMsg(CAN2, 0x1FF, &prvCurrentSendBuf[24], 8);    
}

/** @brief  链表插入 */
static void prvMotorListInsert(Motor_t *pUsartXMotorList, Motor_t *pMotorToInsert)
{
    if(pUsartXMotorList->pNext == NULL){      // 链表为空时第一次插入
        pUsartXMotorList->pNext = pMotorToInsert;
        pUsartXMotorList->pNext->pNext = NULL;
    }else{                                  // 正常插入
        pMotorToInsert->pNext = pUsartXMotorList->pNext;
        pUsartXMotorList->pNext = pMotorToInsert;
    }      
    pUsartXMotorList->LastCounts += 1;  // 此处表示链表头下挂的电机数加一
}
